/*
* StartApplication.java
*
* @author Luis Fernando Ascanio
* @author Parsifal D'Sola
*/
package org.sunspotworld;

import com.sun.spot.core.peripheral.Spot;
import com.sun.spot.core.peripheral.TimeoutException;
import com.sun.spot.core.resources.ResourceUnavailableException;
import com.sun.spot.core.resources.Resources;
import com.sun.spot.core.resources.transducers.IIOPin;
import com.sun.spot.core.resources.transducers.IOutputPin;
import com.sun.spot.core.resources.transducers.ITriColorLED;
import com.sun.spot.core.resources.transducers.LEDColor;
import com.sun.spot.core.util.Utils;
import com.sun.spot.edemo.EDemoBoard;
import com.sun.spot.edemo.peripheral.Servo;
import com.sun.spot.espot.peripheral.ota.OTACommandServer;
import com.sun.spot.ieee_802_15_4_radio.IRadioPolicyManager;
import com.sun.spot.ieee_802_15_4_radio.NoAckException;
import com.sun.spot.ieee_802_15_4_radio.util.IEEEAddress;
import com.sun.spot.multihop.io.j2me.radiogram.Radiogram;
import com.sun.spot.multihop.io.j2me.radiogram.RadiogramConnection;
import edu.ncsu.external_sensors.MaxSonarEZ1UltrasoundSensor;
import edu.ncsu.external_sensors.PINGUltrasoundSensor;
import edu.ncsu.external_sensors.UltrasoundSensor;
import java.io.IOException;
import java.util.Random;
import javax.microedition.io.Connector;
import javax.microedition.io.Datagram;
import javax.microedition.io.DatagramConnection;
import javax.microedition.midlet.MIDlet;
import javax.microedition.midlet.MIDletStateChangeException;
/**
* The startApp method of this class is called by the VM to start the
* application.
*
* The manifest specifies this class as MIDlet-1, which means it will
* be selected for execution.
*/
public class ServoCtrl extends MIDlet {
private IRadioPolicyManager rpm = (IRadioPolicyManager)Resources.lookup(IRadioPolicyManager.class);
private IEEEAddress ourAddr = new IEEEAddress(rpm.getIEEEAddress());
private static final int CHANNEL = 20;  // 16, 20

private static IOutputPin[] hPins = EDemoBoard.getInstance().getOutputPins();
private static Servo wheel1eft = new Servo(hPins[0]);
private static Servo wheelright = new Servo(hPins[1]);

private static IIOPin sensor = EDemoBoard.getInstance().getIOPins()[EDemoBoard.D1];
private static PINGUltrasoundSensor ping = new PINGUltrasoundSensor(sensor,UltrasoundSensor.INCHES);
private static MaxSonarEZ1UltrasoundSensor maxSonar = new MaxSonarEZ1UltrasoundSensor(sensor, UltrasoundSensor.INCHES);
    

private static ITriColorLED [] leds = EDemoBoard.getInstance().getLEDs();
private EDemoBoard demoBoard=EDemoBoard.getInstance();
private static final short PAN_ID = IRadioPolicyManager.DEFAULT_PAN_ID;
private Radiogram rg = null;
private RadiogramConnection conn = null;
private Radiogram xdg = null;
private boolean looking = true;
private boolean connected = false;
private long spotAddress = 0;
private Random random;
private static int[] stop = {1454, 1454};
private static final int STOP_MARGIN = 50;
private int[] low = {stop[0] - STOP_MARGIN, stop[1]-STOP_MARGIN};
private int[] high = {stop[0] + STOP_MARGIN, stop[1]+STOP_MARGIN};
private static final int TC = 27;
private static final double SPEED_OF_SOUND = (331.5 + .6* TC)*39.3700787401575/1000000;  //magic number inches / meter
private static final int MIN_DISTANCE = 10; //distance in inches to obstacle
private static boolean obstacle = false;
    

/** Client command to locate a display server. */
private static final byte LOCATE_SERVER_REQ  = 1;    // sent to display host
/** Host command to indicate it is restarting. */
private static final byte SERVER_RESTART     = 2;
/** Host command to indicate it is quiting. */
private static final byte SERVER_QUITTING    = 3;
 /** Host reply to indicate it is available. */
private static final byte SERVER_AVAIL_REPLY = 11;
    
boolean on=true;
private static boolean moving = false;
int instruct=0,time=0,i=0,sensordist = 0, tiltX, tiltY, ruedaI, ruedaD;

//private IScalarInput[] adcs = EDemoBoard.getInstance().getScalarInputs();
    

private static final byte FORWARD = 0;
private static final byte BACK = 1;
private static final byte LEFT = 2;
private static final byte RIGHT = 3;
private static final byte STOP = 4;

private static byte status=4;

private Thread display = new Thread() {
    public void run() {
        while(true) {
            switch(status) {
                case FORWARD:
                    displayForward();
                    break;
                case BACK:
                    displayBack();
                    break;
                case LEFT:
                    displayLeft();
                    break;
                case RIGHT:
                    displayRight();
                    break;
                case STOP:
                    clearDisplay();
                    break;
                default:
            }
            Utils.sleep(200);
            clearDisplay();
            Utils.sleep(200);
        }
    }
};

private static void displayForward() {
    for (int i = 0; i < 8; i++) {
        leds[i].setColor(LEDColor.GREEN);
        leds[i].setOn();
    }
    obstacle = senseForwardRange() < MIN_DISTANCE;
    if (obstacle) {
        stop();
    }
    else {
        moving = true;
    }
}

private static double senseForwardRange() {
    try {
        if (maxSonar.testConnection()) {
            return maxSonar.getConvertedValue();
        }
        if (ping.testConnection()) {
            return ping.getConvertedValue();
        }
    } catch (ResourceUnavailableException ex) {
        ex.printStackTrace();
    }
    System.out.println("no sensor");
    return 0;
}

private static void displayBack() {
    for (int i = 0; i < 8; i++) {
        leds[i].setColor(LEDColor.RED);
        leds[i].setOn();
    }
    moving = true;
}

private static void displayLeft() {
    for (int i = 0; i < 4; i++) {
        leds[i].setColor(LEDColor.YELLOW);
        leds[i].setOn();
    }
    moving = true;
}

private static void displayRight() {
    for (int i = 4; i < 8; i++) {
        leds[i].setColor(LEDColor.YELLOW);
        leds[i].setOn();
    }
    moving = true;
}

private static void clearDisplay() {
    for (int i = 0; i < 8; i++) {
        leds[i].setOff();
    }
}

/**
     * Wait for a remote SPOT to request a connection.
     */
    private void waitForSpot () {
        RadiogramConnection rcvConn = null;
        DatagramConnection txConn = null;
        spotAddress = 0;
        try {
            rcvConn = (RadiogramConnection)Connector.open("radiogram://:24");
            rcvConn.setTimeout(10000);             // timeout in 10 seconds
            Datagram dg = rcvConn.newDatagram(rcvConn.getMaximumLength());            
            while (true) {
                try {
                    dg.reset();
                    rcvConn.receive(dg);            // wait until we receive a request
                    if (dg.readByte() == LOCATE_SERVER_REQ) {       // type of packet
                        String addr = dg.getAddress();
                        IEEEAddress ieeeAddr = new IEEEAddress(addr);
                        long macAddress = ieeeAddr.asLong();
                        System.out.println("Received request from: " + ieeeAddr.asDottedHex());
                        // txConn = (DatagramConnection)Connector.open("radiogram://" + addr + ":" + BROADCAST_PORT);
                        // Datagram xdg = txConn.newDatagram(txConn.getMaximumLength());
                        Datagram xdg = rcvConn.newDatagram(rcvConn.getMaximumLength());                                dg.reset();
                        xdg.reset();
                        xdg.setAddress(dg);
                        xdg.writeByte(SERVER_AVAIL_REPLY);        // packet type
                        xdg.writeLong(macAddress);                        // requestor's ID
                        rcvConn.send(xdg);                                // broadcast it
                        spotAddress = macAddress;
                        break;
                    }
                } catch (TimeoutException ex) {
                    announceStarting();
                }
            }
        } catch (Exception ex) {
            System.out.println("Error waiting for remote Spot: " + ex.toString());
            ex.printStackTrace();
        } finally {
            try {
                if (rcvConn != null) { 
                    rcvConn.close();
                }
                if (txConn != null) { 
                    txConn.close();
                }
            } catch (IOException ex) { /* ignore */ }
        }
    }

/**
     * Broadcast that the host display server is (re)starting.
     */
    private void announceStarting () {
        DatagramConnection txConn = null;
        try {
            txConn = (DatagramConnection)Connector.open("radiogram://broadcast:24");
            Datagram dg = txConn.newDatagram(txConn.getMaximumLength());
            dg.writeByte(SERVER_RESTART);        // packet type
            txConn.send(dg);                             // broadcast it
        } catch (Exception ex) {
            System.out.println("Error sending display server startup message: " + ex.toString());
            ex.printStackTrace();
        } finally {
            try {
                if (txConn != null) { 
                    txConn.close();
                }
            } catch (IOException ex) { /* ignore */ }
        }
    }

/**
     * Send a simple command request to the remote SPOT.
     *
     * @param cmd the command requested
     */
    private void sendCmd (byte cmd) {
        if (conn != null) {
            try {
                xdg.reset();
                xdg.writeByte(cmd);
                conn.send(xdg);
            } catch (NoAckException nex) {
                connected = false;
            } catch (IOException ex) {
                // ignore any other problems
            }
        }
    }

protected void startApp() throws MIDletStateChangeException {
        rpm.setChannelNumber(CHANNEL);
        display.start();
        //adcs[1].addIScalarInputThresholdListener(new BatteryPackThresholdListener(adcs[1]));
        Spot.getInstance().getSleepManager().disableDeepSleep();
        OTACommandServer.getInstance().setSuspended(false);
//        rpm = Spot.getInstance().getRadioPolicyManager();
        rpm.setPanId(PAN_ID);
        rpm.setOutputPower(31);
        initialize();
        leds[0].setColor(LEDColor.BLUE);               
        leds[1].setColor(LEDColor.BLUE);
        leds[2].setColor(LEDColor.BLUE);
        leds[3].setColor(LEDColor.BLUE);
        leds[4].setColor(LEDColor.BLUE);
        leds[5].setColor(LEDColor.BLUE);
        leds[6].setColor(LEDColor.BLUE);
        leds[7].setColor(LEDColor.BLUE);
        
        announceStarting();
        waitForSpot();

        try {            
            conn = (RadiogramConnection)Connector.open("radiogram://"+ new IEEEAddress(spotAddress).asDottedHex() +":24");
            rg = (Radiogram) conn.newDatagram(conn.getMaximumLength());
            
        } catch (IOException ex) {
            ex.printStackTrace();
        } 
//*/
        ledsOn();

    
//PARA H0(IZQUIERDA)= 2000 ES FORWARD, 1000, BACKWARD, 1489 STOP
//PARA H1(DERECHA)= 1000 ES FORWARD, 2000, BACKWARD, 1488 STOP 

    //EDemoBoard.getInstance().bindOutputPin(EDemoBoard.D3).setHigh();
    int k=0,z;
    while(on){
            try {
                //leds[0].setOn();
                rg.reset();
                conn.receive(rg);
                tiltX = rg.readInt();
                tiltY = rg.readInt();
                
                /*
                if(sensordist<=25){
                    obstacle=true;
                    wall.run();                
                } //*/
                
                
                ruedaI=(tiltY-tiltX)*1000/100+stop[0]; //1488
                ruedaD=(tiltY+tiltX)*1000/100+stop[1]; //1489               
                if (ruedaI > high[0]) {
                    if (ruedaD < low[1]) {
                        status = BACK;
                    } else {
                        status = LEFT;
                    }
                } else if (ruedaI < low[0]) { 
                    if (ruedaD >high[1]) {
                        status = FORWARD;
                    } else {
                        status = RIGHT;
                    }
                } else if (ruedaD > high[1]) {
                    status = LEFT;
                } else if (ruedaD < low[1]) {
                    status = RIGHT;
                } else {
                    status = STOP;
                }
                if(status != STOP && (!obstacle||status !=FORWARD)){
                    wheel1eft.setValue(ruedaI);
                    wheelright.setValue(ruedaD);
                    
                } else {
                    stop();
                }
            } catch (IOException ex) {
                ex.printStackTrace();
            }  
    }
}

    private static void stop() {
        if (moving) {
            brake(250);
        }
        moving = false;
        wheel1eft.setValue(0);
        wheelright.setValue(0);
    }
    
    private static void brake(long time) {
        wheel1eft.setValue(stop[0]);
        wheelright.setValue(stop[1]);
        Utils.sleep(time);
    }

    public void initialize() {
        // currently just use default channel + pan ID
        // Spot.getInstance().getRadioPolicyManager().setChannelNumber(CHANNEL_NUMBER);
        rpm.setPanId(PAN_ID);

        random = new Random(new IEEEAddress(System.getProperty("IEEE_ADDRESS")).asLong());
    }

public void ledsOn(){

    for(int h = 0; h<=7; h++){
        leds[h].setOn();    
    }
}

/**
* This will never be called by the Squawk VM.
*/
protected void pauseApp() {
// This will never be called by the Squawk VM
}
/**
* Only called if startApp throws any exception other than
MIDletStateChangeException.
*/
protected void destroyApp(boolean arg0) throws MIDletStateChangeException {
// Only called if startApp throws any exception other than MIDletStateChangeException
}

    public int getInstruct() {
        return instruct;
    }
}